#include "ros.h"

using namespace std;
using namespace dxy::easyWebServer;
using namespace easyWebServer;


/*
dataInfo 格式
{
    SN_ID: "2023_DXY";
    publisher_pid: 1234,
    publisher_port: 50660,
    subscribe_pids: [1234, 4321, ...],
    subscribe_ports: [50661, 50662, ...],
    topic_name: "/SN_ID/someoneTopic",
    state:  {
                abc: 0,
                xyz: 1
            }
}
*/


// 通过SN号创建ros节点
// SN号注册状态
// 返回通讯端口
void Ros::newConnect()
{
    vector<string> vet;
    boost::split(vet, m_request->get("parameter"), boost::is_any_of("=&"));
    
    Json lvInfo(Json::Type::json_object);
    Json lvState(Json::Type::json_object);

    lvInfo["SN_ID"] = Json(vet[1]);
    for(int i = 2; i < vet.size(); i += 2)
    {
        lvState[vet[i]] = Json(vet[i + 1]);
    }
    lvInfo["state"] = lvState;

    // 创建ros节点
    uint16_t port = createPort();
    pid_t pid = RosNodeManager::getInstance().createPublicRosNode(lvInfo["SN_ID"].asString(), port);
    
    // 等待节点生成
    sleep(1);

    lvInfo["publisher_pid"] = Json((int)pid);
    lvInfo["publisher_port"] = Json((int)port);

    RedisConnector::getInstance().insert(lvInfo["SN_ID"].asString(), lvInfo.str());

    m_response->setHead();
    m_response->add("Content-Length", to_string(lvInfo.str().size()));
    m_response->addContext(lvInfo.str());
}

void Ros::disConnect()
{
    vector<string> vet;
    boost::split(vet, m_request->get("parameter"), boost::is_any_of("=&"));
    RosNodeManager::getInstance().killNode(vet[1]);
    // m_response->setContext("", "200");
    m_response->setHead();
}

uint16_t Ros::createPort()
{
    while(1)
    {
        char psz_port_cmd[256];
        srand(time(NULL));
        uint16_t port = (rand() % 65135) + 4000;
        sprintf(psz_port_cmd, "netstat -an | grep :%d > /dev/null", port);
        int ret = system(psz_port_cmd);
        if(ret == 256)
        {
            return port;
        }
    }
}

void Ros::subNewConnect()
{
    Json responseCtx(Json::Type::json_object);
    Json nodesJson(Json::Type::json_array);
    std::vector<std::string> nodes = RosNodeManager::getInstance().getAllNode();
    for(int i = 0; i < nodes.size(); i++)
    {   
        Json oneNode(Json::Type::json_object);
        oneNode.parse(RedisConnector::getInstance().get(nodes[i]));
        nodesJson.append(oneNode);
    }
    responseCtx["ALL_DEVICE"] = nodesJson;
    m_response->setHead();
    m_response->add("Content-Length", to_string(responseCtx.str().size()));
    m_response->addContext(responseCtx.str());
}

void Ros::changeState()
{
    vector<string> vet;
    boost::split(vet, m_request->get("parameter"), boost::is_any_of("=&"));
    if(vet.size() != 2)
    {
        m_response->setHead();
        m_response->addContext("FAILD");
        return;
    }
    Json nodeMsg(Json::Type::json_object);
    nodeMsg.parse(vet[1]);
    RedisConnector::getInstance().insert(nodeMsg["SN_ID"].asString(), nodeMsg.str());
    m_response->setHead();
    m_response->addContext("OK");
}





